#include "state.h"
#include <iostream>
#include <cstdlib>
using std::cout;
using std::endl;
using std::abs; // int version

void StateDiagram::evaluatePreferredDirTurn(const Position &goal)
{
  int dx = goal.x() - currentState().x();
  int dy = goal.y() - currentState().y();
  // decide the preferred direction and turn
  if (abs(dx) < abs(dy))
    {
      preferredDir_ = (dy < 0) ? DOWN : UP;
      preferredTurn_ = (dx*dy < 0) ? COUNTERCLOCKWISE : CLOCKWISE;
    }
  else // (abs(dx) >= abs(dy))
    {
      preferredDir_ = (dx < 0) ? LEFT : RIGHT;
      preferredTurn_ = (dy*dx < 0) ? CLOCKWISE : COUNTERCLOCKWISE;
    }
}

Position StateDiagram::findNextPosition(const Direction &dir) const
{
  static const size_t STEP = 1; // moving step unit
  size_t next_x = currentState().x(), next_y = currentState().y();
  assert(next_x < map_.size() && next_y < map_.size());
  switch (dir)
    {
    case RIGHT:
      if (next_x + STEP < map_.x_size()) // check the right boundary
	next_x += STEP;
    break;
    case UP:
      if (next_y + STEP < map_.y_size()) // check the upper boundary
	next_y += STEP;
    break;
    case LEFT:
      if (next_x >= STEP) // check the map left boundary
	next_x -= STEP;
    break;
    case DOWN:
      if (next_y >= STEP) // check the map lower boundary
	next_y -= STEP;
    break;
    default:
      assert(!"Unknown moving direction found!" && dir);
    }
  assert(next_x < map_.size() && next_y < map_.size());
  if (BLOCK == map_.at(next_x, next_y)) // check the road block
    return Position(currentState().x(), currentState().y());
  return Position(next_x, next_y);
}

bool StateDiagram::moveTo(const Direction &dir)
{
  Position nextPos(findNextPosition(dir));
  if ((currentState().x() == nextPos.x()) &&
      (currentState().y() == nextPos.y()))
    return false; // Cannot cross over the block or the map boundary
  currentState().dir(dir); // update current state with direction
  if (stateCache_.find(currentState()) != stateCache_.end()) // find a state loop
    {
      cout << "*** Warning: State loop found! Change search stratege." << endl;
      evaluatePreferredDirTurn(map_.goal());
      stateCache_.clear();
      ++has_loop_;
      for (int i = has_loop_; i < NDIRS; ++i)
	preferredDir_ = takeTurn(preferredDir_, preferredTurn_); // Heuristicly update direction
    }
  else
    {
      stateCache_.insert(currentState());
    }
  // add a new state with unknown direction
  path_.push_back(State(nextPos.x(), nextPos.y()));
  // print map with path at this step
  print(std::string("--- Step ") + numToStr(path_.size() - 1));
  return true;
}

bool StateDiagram::searchPath()
{
  cout << "*** Steps to search the path" << endl;
  if (path_.size() > 1) // have found a path already
    return true;
  // Search a start-to-goal path until arriving at the goal
  Direction dir = preferredDir_; // initial moving direction
  while ((currentState().x() != map_.goal().x()) ||
	 (currentState().y() != map_.goal().y()))
    {
      if (path_.size() > 4 * map_.size())
	{
	  cout << "*** Error: Cannot find a path!" << endl;
	  return false;  // Heuristic checking whether there is a solution.
	}
      const Direction prevDir = dir; // save previous direction
      if (numTurns_) // have taken turns before
	{
	  // try reverse turn once at each step
	  dir = takeTurn(dir, reverse(preferredTurn_));
	  if (moveTo(dir))
	    {
	      if (dir == preferredDir_ && !has_loop_) // less evaluation if having had a loop
		{
		  // Have bypassed the obstacle. Reevaluate dir and turn.
		  evaluatePreferredDirTurn(map_.goal());
		  dir = preferredDir_;
		  numTurns_ = 0; // reset the turn counter
		}
	      else // the same as PLEDGE
		{
		  --numTurns_;
		}
	      continue; // goto next while
	    }
	  // failed to try reverse turn
	  dir = prevDir; // recover the direction
	}
      // try to move forward along dir
      for (int dirCnt = 0; dirCnt < NDIRS; ++dirCnt)
	{
	  if (moveTo(dir))
	    break; // jump out of the for-loop
	  dir = takeTurn(dir, preferredTurn_);
	  ++numTurns_; // increase the turn counter
	}
      // the algorithms becomes the Pledge method
      if (dir==preferredDir_ && (has_loop_ < NDIRS)) // when moving forward straightly
	{
	  evaluatePreferredDirTurn(map_.goal()); // reevaluate every time
	  dir = preferredDir_;
	  numTurns_ = 0;
	}
    }
  return true;
}
